/* SPDX-License-Identifier: GPL-2.0+ */
/*
 * Architectural dump for MIPS Core
 *
 * Created by xiaoqzye@qq.com
 *
 * 2020/10/24
 */

#include <asm-offsets.h>
#include <config.h>
#include <asm/asm.h>
#include <asm/regdef.h>
#include <asm/mipsregs.h>

	.macro mips32_read_cp0 regNum
	mfc0      v0, \regNum, 0x0
	jr        ra
	nop
	mfc0      v0, \regNum, 0x1
	jr        ra
	nop
	mfc0      v0, \regNum, 0x2
	jr        ra
	nop
	mfc0      v0, \regNum, 0x3
	jr        ra
	nop
	mfc0      v0, \regNum, 0x4
	jr        ra
	nop
	mfc0      v0, \regNum, 0x5
	jr        ra
	nop
	mfc0      v0, \regNum, 0x6
	jr        ra
	nop
	mfc0      v0, \regNum, 0x7
	jr        ra
	nop
	.endm

	.text
	.set noreorder

ENTRY(mips_dump_cp0)
	addiu     sp, sp, -16   # Allocate stack frame
	sw        s0, 0x4(sp)   # save register s0
	sw        s1, 0x8(sp)   # save register s1
	sw        s2, 0xc(sp)   # save register s2
	move      s0, ra        # backup return address
	move      s1, a2        # backup the third argument
	andi      a0, a0, 0x1f  # register index in range 0~31
	sll       a0, a0, 0x0b  # left shift 11 bits
	andi      a1, a1, 0x07  # the selector, 0~7
	lui       s2, 0x4012    # the upper 16-bit of `mfc0 s2, ...
	addu      s2, s2, a0    # add the CP0 register index
	addu      s2, s2, a1    # Add the selector
	bal       1f            # store the address of `mfc s2, ... into ra
	nop
	mfc0      s2, $12, 0x0  # the to-be modified instruction
	move      ra, s0        # restore the return address register
	lw        s0, 0x4(sp)   # restore register s0
	lw        s1, 0x8(sp)   # restore register s1
	move      v0, s2        # set the return value
	lw        s2, 0xc(sp)   # restore register s2
	jr        ra            # return to caller
	addiu     sp, sp, 16    # restore stack balance
1:
	sw        s2, 0x0(ra)   # modify the `mfc0 s2, ... instruction
	sw        ra, 0x0(s1)   # write the address of instruction
	sw        s2, 0x4(s1)   # write the modified instruction
	lui       a0, %hi(mips_read_cp0)
	addiu     a0, %lo(mips_read_cp0)
	li        a1, 0x80
	j         flush_cache   # call function defined in cache.c
	nop
END(mips_dump_cp0)

ENTRY(mips_read_cp0_)
	nop
	mips32_read_cp0 $0
	mips32_read_cp0 $1
	mips32_read_cp0 $2
	mips32_read_cp0 $3
	mips32_read_cp0 $4
	mips32_read_cp0 $5
	mips32_read_cp0 $6
	mips32_read_cp0 $7
	mips32_read_cp0 $8
	mips32_read_cp0 $9
	mips32_read_cp0 $10
	mips32_read_cp0 $11
	mips32_read_cp0 $12
	mips32_read_cp0 $13
	mips32_read_cp0 $14
	mips32_read_cp0 $15
	mips32_read_cp0 $16
	mips32_read_cp0 $17
	mips32_read_cp0 $18
	mips32_read_cp0 $19
	mips32_read_cp0 $20
	mips32_read_cp0 $21
	mips32_read_cp0 $22
	mips32_read_cp0 $23
	mips32_read_cp0 $24
	mips32_read_cp0 $25
	mips32_read_cp0 $26
	mips32_read_cp0 $27
	mips32_read_cp0 $28
	mips32_read_cp0 $29
	mips32_read_cp0 $30
	mips32_read_cp0 $31
END(mips_read_cp0_)

ENTRY(mips_read_cp0)
	addiu     sp, sp, -24
	sw        ra, 0x04(sp)
	sw        s0, 0x08(sp)
	sw        s1, 0x0c(sp)
	sw        s2, 0x10(sp)

	/* compose the instruction for MIPS32r2 */
	andi      a0, a0, 0x1f
	andi      a1, a1, 0x07
	lui       s0, 0x4002
	sll       s1, a0, 0x0b
	addu      s0, s0, s1
	addu      s0, s0, a1

	/* a0 * 8 * 4 * 3 + a1 * 4 * 3 */
	sll       a0, a0, 0x03
	addu      a0, a0, a1
	sll       a0, a0, 0x02
	sll       a1, a0, 0x01
	addu      a0, a0, a1

	/* get the address of `mips_read_cp0_ */
	lui       a1, %hi(mips_read_cp0_)
	addiu     a1, %lo(mips_read_cp0_)
	addu      s1, a0, a1
	lw        s2, 0x4(s1)

	move      v0, zero
	subu      v1, s0, s2 /* if v1 is non-zero, there is an ERROR */
	jalr      s1         /* invoke the code: mfc0 v0, ... */
	nop
	/* compare the two instructions */
	lw        ra, 0x04(sp)
	lw        s0, 0x08(sp)
	lw        s1, 0x0c(sp)
	lw        s2, 0x10(sp)
	jr        ra
	addiu     sp, sp, 24
END(mips_read_cp0)
